# coding : UTF-8
"""
作者：BingBO   时间：2022年10月23日
自动调整代码格式 ：Alt+Ctrl+L
"""
import cv2 as cv
import numpy as np
import math
import random
from numpy.linalg import solve  # 解线性方程
import moduleCommon as Com
from skimage import morphology  # 形态学操作，如开闭运算、骨架提取等


# 纵向 灰度重心法
def cog(binary, mode, show=None):
    """
    cogV(img) -> retval
    @brief 计算光条中心点坐标，并判断垂直上的断点数量
    .   @param img：输入1通道二值图像.
    .   @retval cp_row_truth：条纹中心点行坐标.
    .   @retval cp_row：条纹中心点列坐标.
    .   @retval cp_col：条纹中心点行坐标.
    .   @retval gap_col：条纹中心点行坐标.
    """
    height, width = binary.shape
    cp_row, cp_col, null_col = [], [], []
    if mode == 0:  # 垂直COG
        for col in range(width):
            x0, y0 = 0, 0
            for row in range(height):
                x0 += binary[row, col]
                y0 += binary[row, col] * (row + 1)
            if x0 != 0:
                indexRow = round(y0 / x0) - 1
                cp_row += [indexRow]
                cp_col += [col]  # 中心点列索引(包含空白区域列坐标的突变)
    elif mode == 1:  # 水平COG
        for row in range(height):
            x0, y0 = 0, 0
            for col in range(width):
                x0 += binary[row, col]
                y0 += binary[row, col] * (col + 1)
            if x0 != 0:
                indexRow = round(y0 / x0) - 1
                cp_col += [indexRow]
                cp_row += [row]  # 中心点列索引(包含空白区域列坐标的突变)
    # print("CP_row:", cp_row)
    # print("CP_col:", cp_col)
    if show == 1:
        # cpGlobal = np.zeros_like(binary)
        cpShow = np.zeros([height, width, 3], np.uint8)
        cpShow[:, :, 0] = binary
        cpShow[:, :, 1] = binary
        cpShow[:, :, 2] = binary
        for i in range(0, len(cp_row)):
            cpShow[cp_row[i], cp_col[i]] = [0, 0, 255]
        cv.imshow("cpShow", cpShow)
        cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\cpShow.jpg", cpShow)
    return cp_row, cp_col


def weldmentExtract(src):
    """
    workpieceExtract(src) -> retval
    @brief 分离出工件上的点集
    .   @param src：输入3通道原始条纹图像.
    .   @retval binary：中值滤波后的阈值图像.
    .   @retval cpweldmentImg：3通道的焊件中心点图像
    .   @retval cpweldment：焊件中心点集[cpweldment_row_truth, cpweldment_row, cpweldment_col]
    """
    print("********************** weldmentExtract **********************")
    blue, green, red = cv.split(src)
    _, binary = cv.threshold(red, 195, 255, cv.THRESH_BINARY)
    binary = cv.medianBlur(binary, 3)
    # cv.imshow('binary', binary)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\binary.jpg", binary)

    gapMaxIdxS, gapMaxIdxL = Com.gapDetection(binary, 0, 0)
    d1 = gapMaxIdxL - gapMaxIdxS
    binaryWeldment = binary[:, 0:int(gapMaxIdxS + d1 / 2)]
    cv.imshow('binaryWeldment', binaryWeldment)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\binaryWeldment.png",
               binaryWeldment)
    return binaryWeldment


def vPointlocation(binaryWeldment):
    # 骨架提取
    NormalizedImg = binaryWeldment / 255  # 像素值归一化
    skeleton = morphology.skeletonize(NormalizedImg)  # 骨架提取
    # skeleton = skeleton.astype(np.uint8) * 255
    # cv.imshow("Skeleton", skeleton)
    # cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\skeleton.png", skeleton)

    cp_row, cp_col = cog(binaryWeldment, 1, show=1)
    # 创建中心点图像
    cpImg = np.zeros_like(binaryWeldment)
    for i in range(0, len(cp_row)):
        cpImg[cp_row[i], cp_col[i]] = 255
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\cpImg.png", cpImg)

    numAll = 0
    for i in range(len(cp_row)):
        row = cp_row[i]
        col = cp_col[i]
        num = 0
        # 统计右上
        for m in range(0, 3):  # 0~3
            for n in range(1, 8):  # 1~9
                if skeleton[row - n, col + m] == 1:
                    num += 1
        for j in range(1, 4):  # 1\2\3
            for p in range(1, 2 * j):
                if skeleton[row - p, col + j] == 1:
                    num -= 1
        # 统计左下
        for u in range(0, 3):  # 0,1,2,3
            for h in range(2 * u, 2 * u + 4):  # 2u~2u+3
                if skeleton[row + u, col - h] == 1:
                    num += 1
        if num > numAll:
            numAll = num
            vPoint = (cp_row[i - 1], cp_col[i - 1])  # 实验证明，选取上一点的精度更高
    print("该点的统计点数：", numAll)
    print("vPoint坐标：", vPoint)
    h, w = binaryWeldment.shape[:2]
    cpImg3 = np.zeros([h, w, 3], np.uint8)
    cpImg3[:, :, 0] = cpImg
    cpImg3[:, :, 1] = cpImg
    cpImg3[:, :, 2] = cpImg
    cpImg3[vPoint[0], vPoint[1]] = [0, 0, 255]
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\vPoint.png", cpImg3)
    return vPoint


def RANSAC(pointsRow, pointsCol):
    """
    RANSAC(pointsRow, pointsCol) -> retval
    @brief
    .   @param pointsRow, pointsCol：点集
    .   @retval Coef_Mainline：拟合直线的Ax+By+C=0的系数 ABC
    """
    print("    ***********RANSAC start*************")
    # RANSAC
    SIZE = len(pointsRow)
    ITERATION = 1000  # 迭代次数
    threshold = 0.8  # 内点数达到总点数的80%
    best_k, best_b = 0, 0  # 最优斜率、最优截距
    innerVertical, innerSlope = 0, 0
    # RANSAC找到左半边点的主线斜率
    for i in range(1, ITERATION + 1):
        sampleIndex = random.sample(range(SIZE), 2)  # 每次迭代都 随机截取两个样本，作为数据索引值
        # 随机取得的两个坐标点(x1，y1)、(x2，y2)
        x1, y1 = pointsCol[sampleIndex[0]], pointsRow[sampleIndex[0]]
        x2, y2 = pointsCol[sampleIndex[1]], pointsRow[sampleIndex[1]]
        if x2 != x1:
            # 随机两点的斜率
            k = (y2 - y1) / (x2 - x1)  # 此处可以改进一下
            b = y1 - k * x1
            innerP1 = 0  # 内点计数器
            for index in range(SIZE):  # 遍历所有数据点
                x, y = pointsCol[index], pointsRow[index]
                d = math.fabs(k * x - y + b) / math.sqrt(1 + k ** 2)
                if d <= 1:
                    innerP1 += 1  # 当前斜率下的所有内点数
            if innerP1 > innerSlope:
                innerSlope = innerP1  # 储存最大的内点数innerSlope和对应斜率
                best_k, best_b = k, b
            # if innerSlope > SIZE * threshold:  # 内点数大于设定的阈值，提前跳出循环
            # break  # 跳出最近一层for、while循环
        else:  # x2 == x1
            innerP2 = 0  # 内点计数器2
            for index in range(SIZE):  # 遍历所有数据点: cpSection_col[0] ~ cpSection_col[SIZE-1]
                x, y = pointsCol[index], pointsRow[index]
                if x == x1:
                    innerP2 += 1
            if innerP2 > innerVertical:
                innerVertical = innerP2  # 储存最大的内点数innerVertical
                C = -x2
            # if innerVertical > SIZE * threshold:  # 内点数大于设定的阈值，提前跳出循环
            # break  # 跳出最近一层for、while循环
    print("RANSAC迭代 %d 次" % i)
    if innerVertical >= innerSlope:
        A, B, C = 1, 0, C
    else:
        A, B, C = best_k, -1, best_b
    Coef_Mainline = [A, B, C]
    print("(RANSAC)点集拟合直线ABC系数：", Coef_Mainline)
    print("     ***********RANSAC End*************")
    return Coef_Mainline


def grooveL_V(cpweldmentImg, fpLeft):
    """
    grooveL_V(fpLeftImg, fpLeft) -> retval
    @brief  分割出左边坡口上的点集grooveLeft，
    .   @param fpLeftImg：最远点左点集图像（3通道）
    .   @param fpLeft：最远点左点集(行，列) fpLeft = [fpLeft_row, fpLeft_col]
    .   @retval grooveLeftImg：左边坡口图（3通道）
    .   @retval grooveLeft：左边坡口上的点集
    .   @retval polyCoeffi_fpLeft：[最远点左边主线系数 polyCoef_fpLeftMainline, 左坡口系数 polyCoef_grooveLeft]
    """
    print("\n**********************grooveL_V**********************")
    fpLeft_row, fpLeft_col = fpLeft
    # RANSAC
    Coef_fpLeftMainline = RANSAC(fpLeft_row, fpLeft_col)
    best_k, _, best_b = Coef_fpLeftMainline  # A1, B1, C1 = best_k, -1, best_b
    # print("(RANSAC)最远点左边点 直线ABC系数：", polyCoef_fpLeftMainline)
    x1, x2 = fpLeft_col[0], fpLeft_col[-1]
    y1, y2 = round(best_b + best_k * x1), round(best_b + best_k * x2)
    cv.line(cpweldmentImg, (x1, y1), (x2, y2), (0, 255, 0), 1)

    # 计算各个中心点到主线的距离，找出左边坡口上的点
    grooveL_row, grooveL_col = [], []
    count = 0
    for i in range(0, len(fpLeft_row)):
        d = math.fabs(best_k * fpLeft_col[i] - fpLeft_row[i] + best_b) / math.sqrt(1 + best_k ** 2)
        if d >= 3:
            grooveL_row += [fpLeft_row[i]]
            grooveL_col += [fpLeft_col[i]]
            count += 1
    print("grooveLeft_row:", grooveL_row)
    print("grooveLeft_col:", grooveL_col)
    print("左坡口共找到%d个点" % count)

    grooveLeft = [grooveL_row, grooveL_col]

    # 在cpweldmentImg显示左坡口中心点
    for i in range(0, len(grooveL_row)):
        cpweldmentImg[grooveL_row[i], grooveL_col[i]] = [255, 0, 0]  # 左坡口上点蓝色

    grooveL_k, grooveL_b = np.polyfit(grooveL_col, grooveL_row, 1)
    # 显示左坡口拟合线
    x1, x2 = grooveL_col[0], grooveL_col[-1]
    y1, y2 = round(grooveL_b + grooveL_k * x1), round(grooveL_b + grooveL_k * x2)
    cv.line(cpweldmentImg, (x1, y1), (x2, y2), (0, 255, 0), 1)

    Coef_grooveL = grooveL_k, -1, grooveL_b
    print("(最小二乘拟合)左坡口 直线ABC系数：", Coef_grooveL)
    Coef_fpLeft = [Coef_fpLeftMainline, Coef_grooveL]

    return grooveLeft, Coef_fpLeft  # 返回：最远点左边点、左坡口直线拟合系数ABC


def cogH_Scetion(img, RowSection):
    """
    cogH_Scetion(img, SectionRow) -> retval
    @brief  分割出左边坡口上的点集grooveLeft，
    .   @param img：单通道二值图像
    .   @param RowSection：横向行区间
    .   @retval
    """
    _, width = img.shape
    SectionRow_Min, SectionRow_Max = RowSection[0] + 3, RowSection[1] - 3  # RowSection在RANSAC_Right-COG_U()取到
    cpSection_row, cpSection_col = [], []
    for row in range(SectionRow_Min, SectionRow_Max + 1):  # 遍历区间每一行
        sumx, sumy = 0, 0
        for col in range(width):  # 遍历这一行的每一列
            sumx += img[row, col]
            sumy += img[row, col] * (col + 1)  # 重心在第几列
        if sumx != 0:
            indexCol = (sumy / sumx) - 1  # 化为坐标索引
            cpSection_row += [row]  # r值是从小到大的
            cpSection_col += [round(indexCol)]  # 从小到大（右下）、从大到小（右上）
    cpSection = [cpSection_row, cpSection_col]
    return cpSection


# 横向 灰度重心法
def largeSlopeExtract(grooveRSection):  # grooveRSection = min(grooveR_row), max(grooveR_row)
    binary = cv.imread(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\binary.jpg")
    binary = binary[:, :, 0]
    cpSection = cogH_Scetion(binary, grooveRSection)
    cpSection_row, cpSection_col = cpSection

    print("COG_U共找到右坡口区间上的中心点个数:", len(cpSection_row))
    print("cpSection_row:", cpSection_row)
    print("cpSection_col:", cpSection_col)
    # RANSAC获取右坡口直线参数
    Coef_Mainline = RANSAC(cpSection_row, cpSection_col)
    return cpSection, Coef_Mainline  # 返回：区间中心点集、右坡口直线拟合系数ABC


def grooveR_V(cpweldmentImg, fpRight):
    """
    grooveR_V(fpLeftImg, fpLeft) -> retval
    @brief  分割出右边坡口上的点集grooveRight，
    .   @param fpRightImg：最远点右边点集图像（3通道）
    .   @param fpRight：最远点右边点集(行，列) fpRight = [fpRight_row_truth, fpRight_row, fpRight_col]
    .   @retval grooveLeftImg：左边坡口图（3通道）
    .   @retval grooveRight：左边坡口上的点集
    .   @retval polyCoeffi_fpLeft：[最远点左边主线系数 polyCoef_fpLeftMainline, 左坡口系数 polyCoef_grooveLeft]
    """
    print("\n**********************grooveR_V**********************")
    fpR_row, fpR_col = fpRight
    # RANSAC
    Coef_fpRightMainline = RANSAC(fpR_row, fpR_col)
    best_k, _, best_b = Coef_fpRightMainline  # A1, B1, C1 = best_k, -1, best_b
    # print("(RANSAC)最远点右边点 直线ABC系数：", polyCoef_fpRightMainline)
    x1, x2 = fpR_col[0], fpR_col[-1]
    y1, y2 = round(best_b + best_k * x1), round(best_b + best_k * x2)
    cv.line(cpweldmentImg, (x1, y1), (x2, y2), (0, 255, 0), 1)

    # 计算各个中心点到主线的距离，找出右边坡口上的点
    grooveR_row, grooveR_col = [], []
    count = 0
    for i in range(0, len(fpR_col)):
        d = math.fabs(best_k * fpR_col[i] - fpR_row[i] + best_b) / math.sqrt(1 + best_k ** 2)
        if d >= 2.5:
            grooveR_row += [fpR_row[i]]
            grooveR_col += [fpR_col[i]]
            count += 1
    print("grooveRight_row:", grooveR_row)
    print("grooveRight_col:", grooveR_col)
    print("右坡口共找到%d个点" % count)

    grooveRight = [grooveR_row, grooveR_col]
    # 在cpweldmentImg显示右坡口中心点
    for i in range(0, len(grooveR_row)):
        cpweldmentImg[grooveR_row[i], grooveR_col[i]] = [255, 255, 255]

    # 用最小二乘法（含COG_V误差）拟合得到的直线参数
    lsm_k, lsm_b = np.polyfit(grooveR_col, grooveR_row, 1)
    Coef_grooveRight = [lsm_k, -1, lsm_b]
    if math.fabs(lsm_k) > 1.8:  # 当斜率过大时候，误差较大，调用COG_U
        grooveRSection = [min(grooveR_row), max(grooveR_row)]
        cpSection, Coef_grooveRight = largeSlopeExtract(grooveRSection)  # 更新polyCoeffiABC_grooveRight
        cpSection_row, cpSection_col = cpSection
        grooveRight = cpSection  # 更新grooveRight坐标
        Ar, Br, Cr = Coef_grooveRight
        if Br == 0:  # pV_num直线上的点 >= pK_num斜线上的点
            print("右坡口为垂直光条 (RANSAC)Ar, Br, Cr =", (Ar, Br, Cr))
            # 绘制图像 —— cpSection_row始终是从小到大的
            y1, y2 = min(grooveR_row) - 10, max(grooveR_row) + 10
            cv.line(cpweldmentImg, (round(Cr), y1), (round(Cr), y2), (0, 255, 0), 1)
        elif Br == -1:
            print("右坡口为斜率光条 (RANSAC)Ar, Br, Cr =", (Ar, Br, Cr))
            # 绘制图像cpSection_col大小顺序不确定
            best_k, best_b = Ar, Cr
            x1, x2 = round(min(cpSection_col) - 10), round(max(cpSection_col) + 10)
            y1, y2 = round(best_b + best_k * x1), round(best_b + best_k * x2)
            cv.line(cpweldmentImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    else:  # 否则使用最小二乘法的拟合参数
        x1, x2 = grooveR_col[0], grooveR_col[-1]
        y1, y2 = round(lsm_b + lsm_k * x1), round(lsm_b + lsm_k * x2)
        cv.line(cpweldmentImg, (x1, y1), (x2, y2), (0, 255, 0), 1)

    cv.imshow("last cpweldmentImg", cpweldmentImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\last cpweldment.png",
               cpweldmentImg)
    Coeffi_fpRight = [Coef_fpRightMainline, Coef_grooveRight]
    return grooveRight, Coeffi_fpRight


# 计算各个特征点
def cpImgDisplay(Img, fpLeft, fpRight, grooveLeft, grooveRight, Coeffi_fpLeft, Coeffi_fpRight):
    print("\n**********************cpImgDisplay**********************")
    # 四部分的 点集坐标
    fpL_row, fpL_col = fpLeft
    fpR_row, fpR_col = fpRight
    grooveL_row, grooveL_col = grooveLeft
    grooveR_row, grooveR_col = grooveRight

    # 四部分的 拟合直线系数
    Coef_fpLeftMainline, Coef_grooveLeft = Coeffi_fpLeft
    Coef_fpRightMainline, Coef_grooveRight = Coeffi_fpRight

    Al, Bl, Cl = Coef_fpLeftMainline
    Agl, Bgl, Cgl = Coef_grooveLeft
    Agr, Bgr, Cgr = Coef_grooveRight
    Ar, Br, Cr = Coef_fpRightMainline
    print("Agr, Bgr, Cgr:", Agr, Bgr, Cgr)
    # 绘图
    x1, x2 = fpL_col[0], grooveL_col[0] + 12
    x3, x4 = grooveL_col[0] - 40, grooveL_col[-1] + 20
    x7, x8 = fpR_col[0] - 10, fpR_col[len(fpR_col) - 1]

    y1 = round((-Cl - Al * x1) / Bl)
    y2 = round((-Cl - Al * x2) / Bl)
    cv.line(Img, (x1, y1), (x2, y2), (0, 255, 255), 1)  # 黄色

    y3 = round((-Cgl - Agl * x3) / Bgl)
    y4 = round((-Cgl - Agl * x4) / Bgl)
    cv.line(Img, (x3, y3), (x4, y4), (34, 139, 34), 2)  # 绿色

    if Bgr == 0:
        y5 = grooveR_row[0] - 10
        y6 = grooveR_row[-1] + 10
        cv.line(Img, (-Cgr, y5), (-Cgr, y6), (255, 144, 30), 2)  # 粉红
    else:
        x5, x6 = grooveR_col[0] - 5, grooveR_col[-1] + 10
        y5 = round((-Cgr - Agr * x5) / Bgr)
        y6 = round((-Cgr - Agr * x6) / Bgr)
        cv.line(Img, (x5, y5), (x6, y6), (255, 144, 30), 2)

    y7 = round((-Cr - Ar * x7) / Br)
    y8 = round((-Cr - Ar * x8) / Br)
    cv.line(Img, (x7, y7), (x8, y8), (255, 255, 0), 1)  # 青蓝

    # 计算3个特征点坐标
    A1 = np.array([[Al, Bl], [Agl, Bgl]])  # 输出系数矩阵A
    b1 = np.array([-Cl, -Cgl])  # 值
    fp_x1, fp_y1 = solve(A1, b1)
    fp_y1, fp_x1 = round(fp_y1), round(fp_x1)  # a, b = int(x), int(y)  # 直接省略小数部分
    # cpImg[fp_y1, fp_x1] = [255, 0, 255]
    cv.circle(Img, [fp_x1, fp_y1], 1, (255, 0, 255), -1)
    print("特征点1坐标：", (fp_y1, fp_x1))

    A2 = np.array([[Agl, Bgl], [Agr, Bgr]])  # 输出系数矩阵A
    b2 = np.array([-Cgl, -Cgr])  # 值
    fp_x2, fp_y2 = solve(A2, b2)
    fp_y2, fp_x2 = round(fp_y2), round(fp_x2)
    # cpImg[fp_y2, fp_x2] = [255, 0, 255]
    cv.circle(Img, [fp_x2, fp_y2], 1, (255, 0, 255), -1)
    print("特征点2(焊缝)坐标：", (fp_y2, fp_x2))

    A3 = np.array([[Agr, Bgr], [Ar, Br]])  # 输出系数矩阵A
    b3 = np.array([-Cgr, -Cr])  # 值
    fp_x3, fp_y3 = solve(A3, b3)
    fp_y3, fp_x3 = round(fp_y3), round(fp_x3)
    # cpImg[fp_y3, fp_x3] = [255, 0, 255]
    cv.circle(Img, [fp_x3, fp_y3], 1, (255, 0, 255), -1)
    print("特征点3坐标：", (fp_y3, fp_x3))

    # cv.imshow("cpImgDisplay", cpImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\cpImgDisplay.png", Img)
    return fp_y2, fp_x2


def slipVector(pointSet, mode=None):
    point_row, point_col = pointSet
    pointsSet = []
    for i in range(len(point_row)):
        pointsSet.append((point_row[i], point_col[i]))
    print("pointsSet:", pointsSet)

    ls1 = [1, 2, 3, 3, 4, 5, 6, 7, 8]
    ls2 = [-1, -2, -3, -4, -5, -6, -7, -8, -9]
    list = []
    for i in range(len(point_row)):
        row = point_row[i]
        col = point_col[i]
        num = 0
        for k in range(len(ls1)):
            if (row + ls1[k], col + ls2[k]) in pointsSet:  # 以该点为起点的向量，在点集中的数量
                num += 1
        list += [num]
    print(list)

"""
def farthestPoint(setX, setY):

    farthestPoint(etX, setY) -> retval
    @brief 计算各个中心点到主线的距离，找出最远特征点的坐标(不适用于搭接、对接)
    .   @param setX：列坐标.
    .   @param setY：真实行坐标.
    .   @retval polyCoef：最小二乘法拟合系数.
    .   @retval farthestIndex：最远点的索引.

    polyCoef = np.polyfit(setX, setY, 1)  # np.polyfit(x,y,num)：对一组数据进行多项式拟合
    k, b = polyCoef
    dMax = 0
    for i in range(0, len(setX)):
        d = math.fabs(k * setX[i] - setY[i] + b) / math.sqrt(1 + k ** 2)  # 主线方程：kx-y+b=0
        if d >= dMax:
            dMax = d
            farthestIndex = i
    return polyCoef, farthestIndex


# 对工件中心点，最小二乘法拟合主线，提取最远点，分割左右点集
def cpDivision(cpweldmentImg, cpweldment):

    cpDivision(etX, setY) -> retval
    brief
    .   param cpweldmentImg：3通道的焊件中心点图像
    .   param cpweldment：中心点集[cpweldment_row, cpweldment_col]
    .   retval fpLeft，fpRight：最远点左右点集(行，列)
    .   retval fpLeftImg, fpRightImg：最远点左右点集图像（3通道）

    print("\n********************** cpDivision **********************")
    cpweldment_row, cpweldment_col = cpweldment
    polyCoef, Index = farthestPoint(cpweldment_col, cpweldment_row)
    k, b = polyCoef
    print("(最小二乘拟合)焊件点集 直线斜率k：", k)
    print("(最小二乘拟合)焊件点集 直线截距b：", b)

    x1, x2 = cpweldment_col[0], cpweldment_col[-1]
    y1, y2 = round(b + k * x1), round(b + k * x2)
    cv.line(cpweldmentImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    # cv.imshow("cpweldment Fitline Img", cpWorkPieceImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\cpweldmentfit.png", cpweldmentImg)

    coor = (cpweldment_row[Index], cpweldment_col[Index])
    print("距离主线最远中心点坐标：", coor)
    cpweldmentImg[coor[0], coor[1]] = [0, 0, 255]  # 标记最远点
    # cv.imshow("furthest Point", cpweldmentImg)
    # cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\furthestPoint.png", img)  # 保存图像

    # ***********分割最远点左右点集*************
    fpLeft_row, fpLeft_col = cpweldment_row[:Index + 1], cpweldment_col[:Index + 1]
    fpRight_row, fpRight_col = cpweldment_row[Index + 1:], cpweldment_col[Index + 1:]

    fpLeft = [fpLeft_row, fpLeft_col]
    fpRight = [fpRight_row, fpRight_col]
    return fpLeft, fpRight
"""
